//
// Created by xiang on 22-11-10.
//

#include <gflags/gflags.h>
#include <glog/logging.h>

#include "ch8/lio-preinteg/lio_preinteg.h"
#include "common/io_utils.h"
#include "common/sys_utils.h"
#include "common/timer/timer.h"

DEFINE_string(file_path, "/home/zl/Public/kaist_data/urban_data/", "数据文件路径");
DEFINE_string(txt_path, "/home/zl/Public/kaist_data/urban_data/sensor_data/data_stamp.csv", "数据文件路径");
DEFINE_string(config, "../config/velodyne_kaist.yaml", "path of config yaml");  // 配置文件类型
DEFINE_bool(display_map, true, "display map?");

int main(int argc, char** argv) {
    google::InitGoogleLogging(argv[0]);
    FLAGS_stderrthreshold = google::INFO;
    FLAGS_colorlogtostderr = true;
    google::ParseCommandLineFlags(&argc, &argv, true);

    sad::LioPreinteg lio;
    lio.Init(FLAGS_config);

    sad::KaistCsvIO io(FLAGS_file_path, FLAGS_txt_path);
    io.SetIMUProcessFunc([&](const sad::IMU& imu) {
          lio.IMUCallBack(std::shared_ptr<sad::IMU>(new sad::IMU(imu)));                  
          usleep(1e3);
    })         
        .SetGNSSProcessFunc([&](const sad::GNSS& gnss) {

    })
        .SetOdomProcessFunc([&](const sad::Odom& odom) {
          lio.OdomCallBack(std::shared_ptr<sad::Odom>(new sad::Odom(odom)));
    })
        .SetSimplePointCloudProcessFunc([&](const pcl::PointCloud<SimplePointType>::Ptr& cloud_ptr) -> bool  {
            common::Timer::Evaluate([&]() { 
                //LOG(INFO) << "cloud_ptr : " << cloud_ptr->size();
                lio.PCLCallBack(cloud_ptr); 
            }, 
            "preinteg lio");
            return true;
    })
    .Go();

    lio.Finish();
    common::Timer::PrintAll();
    LOG(INFO) << "done.";

    return 0;
}
